/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * GenericVirtualMotor : This class allow differential drive motor simulation.
 **/
package org.hypheli.testApps;

import java.text.DecimalFormat;
import java.text.NumberFormat;

import org.hh.common.HHObject;
import org.hh.common.KeyPoint;
import org.hh.common.MapManager;

import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;

import com.googlecode.javacpp.BytePointer;
import com.googlecode.javacv.cpp.opencv_core;
import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_core.CvPoint;
import com.googlecode.javacv.cpp.opencv_core.CvScalar;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
import com.googlecode.javacv.cpp.opencv_imgproc;

import ioio.lib.api.AnalogInput;
import ioio.lib.api.IOIO;
import ioio.lib.api.exception.ConnectionLostException;

/**
mm  V     1/V
004 1.370 0.730
010 1.844 0.542
020 2.222 0.450
026 2.732 0.366
030 3.022 0.331
036 2.970 0.337
040 2.722 0.367
050 2.332 0.429
060 2.010 0.498
070 1.754 0.570
080 1.554 0.644
090 1.400 0.714
100 1.254 0.797
120 1.046 0.956
140 0.932 1.073
160 0.822 1.217
180 0.733 1.364
200 0.655 1.527
250 0.533 1.876
300 0.433 2.309
350 0.377 2.653
400 0.311 3.215


**/

public class SharpGP2D12 extends HHObject {

	private final static float [][] fLut = new float[][]{
		/* 1/V	 meter */
		new float[]{0.730f, 0.004f}, /*  0 */
		new float[]{0.542f, 0.010f}, /*  1 */
		new float[]{0.450f, 0.020f}, /*  2 */
		new float[]{0.366f, 0.026f}, /*  3 */
		new float[]{0.331f, 0.030f}, /*  4 */
		new float[]{0.337f, 0.036f}, /*  5 */
		new float[]{0.367f, 0.040f}, /*  6 */
		new float[]{0.429f, 0.050f}, /*  7 */
		new float[]{0.498f, 0.060f}, /*  8 */
		new float[]{0.570f, 0.070f}, /*  9 */
		new float[]{0.644f, 0.080f}, /* 10 */
		new float[]{0.714f, 0.090f}, /* 11 */
		new float[]{0.797f, 0.100f}, /* 12 */
		new float[]{0.956f, 0.120f}, /* 13 */
		new float[]{1.073f, 0.140f}, /* 14 */
		new float[]{1.217f, 0.160f}, /* 15 */
		new float[]{1.364f, 0.180f}, /* 16 */
		new float[]{1.527f, 0.200f}, /* 17 */
		new float[]{1.876f, 0.250f}, /* 18 */
		new float[]{2.309f, 0.300f}, /* 19 */
		new float[]{2.653f, 0.350f}, /* 20 */
		new float[]{3.215f, 0.400f}  /* 21 */
	};

	
  private double fDistanceDetection = 0.;
  private boolean fIsSimulation = false;
	private int fPin = 0;
	private IOIO fIoio = null;
	private KeyPoint fPosition = new KeyPoint();
		
	private AnalogInput fAnalogInput = null;

	private double fMeterToPix;
  private KeyPoint fCurrPos;
  
  private CvMat fArr = null;
  
  NumberFormat fNumberFormat;
  
  float fVoltage = 0.f;
    
	public SharpGP2D12(String name, int pin, double px, double py, double pa, double meterToPix){
		super(name);
		
	  fPin = pin;
	  fMeterToPix = meterToPix;
	  fPosition.x = px;
	  fPosition.y = py;
	  fPosition.a = pa;
	  	  
	  fArr = opencv_core.cvCreateMat((int)(meterToPix * fLut[21][1] * 1.5 + 1), 1, opencv_core.CV_8U);
	  
	  fNumberFormat = new DecimalFormat("0.000");
	  
	}

	protected void finalize() throws Throwable
	{
		if( fAnalogInput != null)
			fAnalogInput.close();
	  super.finalize(); //not necessary if extending Object.
	} 
	
	public void initIOIO(boolean isSimulation, IOIO ioio){
		fIsSimulation = isSimulation;

		if(fIsSimulation) return;
		
		fIoio = ioio;
		try {
			fAnalogInput = fIoio.openAnalogInput(fPin);
		} catch (ConnectionLostException e) {
			e.printStackTrace();
		};
		
	}
	
	public void updateIOIO(){
		if( fIsSimulation ) return;
		
		if( fIoio == null || 
				fAnalogInput == null ||
				fCurrPos == null
				) return;
		
		fVoltage =  1.f/fLut[21][0];
		try {
			fVoltage = fAnalogInput.getVoltage();
		} catch (ConnectionLostException e) {
			e.printStackTrace();
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		synchronized(this) {
			fDistanceDetection = convertVolt2Meter(fVoltage);
		}
	}

	public void setCurrentPosition(KeyPoint currPos)
	{		
		fCurrPos = currPos;
	}

	public void onDraw(Canvas canvas, Paint paint){
		if( fCurrPos == null) return;

		
		KeyPoint p = new KeyPoint();
		p.x = fPosition.x;
		p.y = fPosition.y;
		p.a = fPosition.a;
		CvPoint p1 = MapManager.rotationPoint(
				fPosition.x + fCurrPos.x, 
				fPosition.y + fCurrPos.y, 
				fCurrPos, fCurrPos.a, fMeterToPix);
		
		synchronized(this) {
			p.x += fDistanceDetection;
		}
		p.rotate(fPosition, fPosition.a);
		CvPoint p2 = MapManager.rotationPoint(
				p.x + fCurrPos.x, 
				p.y + fCurrPos.y, 
				fCurrPos, fCurrPos.a, fMeterToPix);
		
		
		paint.setColor(Color.RED);
		canvas.drawLine(p1.x(), p1.y(), p2.x(), p2.y(), paint);
		/*
		paint.setColor(Color.BLACK);
		String posString = fNumberFormat.format(fDistanceDetection);
  	canvas.drawText(posString, p2.x(), p2.y(), paint);
  	posString = fNumberFormat.format(fVoltage);
  	canvas.drawText(posString, p2.x(), p2.y()+10, paint);
		 */

	}

	synchronized public double getDistanceDetected(){
		return fDistanceDetection;
	}

	public void drawNoObstacleLine(Canvas canvas, Paint paint, IplImage probaImg, int zoomFactor){
		if( fCurrPos == null) return;

		//boolean foundSomething = false;
		KeyPoint p = new KeyPoint();
		p.x = fPosition.x;
		p.y = fPosition.y;
		p.a = fPosition.a;
		CvPoint p1 = MapManager.rotationPoint(
				fPosition.x + fCurrPos.x, 
				fPosition.y + fCurrPos.y, 
				fCurrPos, fCurrPos.a, fMeterToPix/zoomFactor);
		
		synchronized(this) {
			p.x += fDistanceDetection;
		};
		p.rotate(fPosition, fPosition.a);
		CvPoint p2 = MapManager.rotationPoint(
				p.x + fCurrPos.x, 
				p.y + fCurrPos.y, 
				fCurrPos, fCurrPos.a, fMeterToPix/zoomFactor);
		
		
		if(fIsSimulation){
			
			double distDetect = fLut[21][1];

			CvPoint p1bis = MapManager.rotationPoint(
					fPosition.x + fCurrPos.x, 
					fPosition.y + fCurrPos.y, 
					fCurrPos, fCurrPos.a, fMeterToPix/zoomFactor);
			

			p.x = fPosition.x + distDetect;
			p.y = fPosition.y;
			p.a = fPosition.a;
			
			p.rotate(fPosition, fPosition.a);
			CvPoint p2bis = MapManager.rotationPoint(
					p.x + fCurrPos.x, 
					p.y + fCurrPos.y, 
					fCurrPos, fCurrPos.a, fMeterToPix/zoomFactor);


			// int cvSampleLine(CvArr image, CvPoint pt1, CvPoint pt2, Pointer buffer, int connectivity);
			int x = p1bis.x();
			int y = p1bis.y();
			if( x < 0 ) x = 0;
			if( y < 0 ) y = 0;
			if( x > probaImg.width() ) x = probaImg.width();
			if( y > probaImg.height() ) y = probaImg.height();
			p1bis.set(x, y);
			
			x = p2bis.x();
			y = p2bis.y();
			if( x < 0 ) x = 0;
			if( y < 0 ) y = 0;
			if( x > probaImg.width() ) x = probaImg.width();
			if( y > probaImg.height() ) y = probaImg.height();
			p2bis.set(x, y);
			
			BytePointer iii = fArr.data_ptr();
			int lineNb = opencv_imgproc.cvSampleLine(probaImg, p1bis, p2bis, iii, 8);


			int val = 0;
		  for(int i = 0; i < lineNb; i++)
		  {
		  	Byte r = iii.get(i);
		  	val = r;
		  	if( r < 0){ // unwrap byte value
		  		val = 256 + r;
		  	}
		  	if( val >= 200 ){
		  		distDetect = (double)((i-2) * zoomFactor) / fMeterToPix;
		  		p2bis.set( p1bis.x() + (p2bis.x() - p1bis.x()) * (i-2) / lineNb,
		  						p1bis.y() + (p2bis.y() - p1bis.y()) * (i-2) / lineNb);
		  		break;
		  	}
		  }
		  p2.set(p2bis);
			synchronized(this) {
				fDistanceDetection = distDetect;
			}
		}

	  
		opencv_core.cvLine(probaImg, p1, p2, CvScalar.BLACK, 1, 1/*opencv_core.CV_AA*/, 0);
		if( !fIsSimulation && fDistanceDetection < fLut[fLut.length-1][1]){
			opencv_core.cvSet2D(probaImg, p2.y(), p2.x(), CvScalar.WHITE);
		}
		

		 
		 
	}
	
	private double convertVolt2Meter(double valueInVolt)
	{	
		
		float vinv = (float) (1.f / valueInVolt);
		int i = fLut.length-1;
		float distance = fLut[i][1];
		while( i > 0){
			if( vinv <= fLut[i][0] && vinv > fLut[i-1][0]){
				float fact = 1.f - (fLut[i][0] - vinv) / (fLut[i][0] - fLut[i-1][0]);
				distance = fLut[i-1][1] + fact * (fLut[i][1] - fLut[i-1][1]);
			}
			i--;
		}
		return distance;
	}
	
}
